#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <cv.h>
#include <highgui.h>
#include <opencv2/aruco.hpp>
#include <stdio.h>

#include <vector>
#include <string>
#include <mutex>
#include <iostream>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <acupoint_msgs/AcupointImagePixel.h>
#include <acupoint_msgs/AcupointCoordinate.h>
#include <acupoint_msgs/LegCoordinate.h>

#include<Eigen/Core>
#include<Eigen/Dense>


const std::string LEFT_UPPER="l_BL40";
const std::string LEFT_LOWER="l_BL59";
const std::string RIGHT_UPPER="r_BL40";
const std::string RIGHT_LOWER="r_BL59";


ros::Publisher left_leg_pub,right_leg_pub;

void acupointCallback(const acupoint_msgs::AcupointCoordinateConstPtr &msg){
    int left_upper_index=-1;
    int left_lower_index=-1;
    int right_upper_index=-1;
    int right_lower_index=-1;
    for(int i=0;i<msg->name.size();i++){
        if(msg->name[i]==LEFT_UPPER){
            left_upper_index=i;
        }
        if(msg->name[i]==LEFT_LOWER){
            left_lower_index=i;
        }
        if(msg->name[i]==RIGHT_UPPER){
            right_upper_index=i;
        }
        if(msg->name[i]==RIGHT_LOWER){
            right_lower_index=i;
        }
    }
    acupoint_msgs::LegCoordinate left_leg_msg,right_leg_msg;
    if(left_upper_index!=-1 && left_lower_index!=-1){
        left_leg_msg.header.stamp=ros::Time::now();
        left_leg_msg.position.resize(2);
        left_leg_msg.position[0].x=msg->acupoints[left_upper_index].x;
        left_leg_msg.position[0].y=msg->acupoints[left_upper_index].y;
        left_leg_msg.position[0].z=msg->acupoints[left_upper_index].z;
        left_leg_msg.position[1].x=msg->acupoints[left_lower_index].x;
        left_leg_msg.position[1].y=msg->acupoints[left_lower_index].y;
        left_leg_msg.position[1].z=msg->acupoints[left_lower_index].z;
        left_leg_pub.publish(left_leg_msg);
    }
    if(right_upper_index!=-1 && right_lower_index!=-1){
        right_leg_msg.header.stamp=ros::Time::now();
        right_leg_msg.position.resize(2);
        right_leg_msg.position[0].x=msg->acupoints[right_upper_index].x;
        right_leg_msg.position[0].y=msg->acupoints[right_upper_index].y;
        right_leg_msg.position[0].z=msg->acupoints[right_upper_index].z;
        right_leg_msg.position[1].x=msg->acupoints[right_lower_index].x;
        right_leg_msg.position[1].y=msg->acupoints[right_lower_index].y;
        right_leg_msg.position[1].z=msg->acupoints[right_lower_index].z;   
        right_leg_pub.publish(right_leg_msg);
    }
}


int main(int argc, char *argv[])

{
    ros::init(argc, argv, "get_leg_line_coordinate");
    ros::NodeHandle nh;

    ros::Subscriber acu2base_sub=nh.subscribe<acupoint_msgs::AcupointCoordinate>("/acupoint/acu2base_coordinate",1,acupointCallback);
    left_leg_pub=nh.advertise<acupoint_msgs::LegCoordinate>("/acupoint/left_leg_line",100);
    right_leg_pub=nh.advertise<acupoint_msgs::LegCoordinate>("/acupoint/right_leg_line",100);

    ros::spin();
    return 0;
}




